﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Diagnostics;
using MotionPlanning.MotionPlanningPrimitives;
using MotionPlanning.MotionPlanningComponents;
using MotionPlanning.MotionPlanningComponents.Visualizations;
using MotionPlanning.MotionPlanningComponents.MotionPlanningSystems;
using System.Threading;

namespace MotionPlanning.MotionPlanningAlgorithms.RRT_Standard
{
    ///
    /// Standard RRT
    ///
    public class RRT : Planner_RRTBased
    {
        public override Node SpawnNode_Like(double[] q)
        {
            if (q == null)
                throw new Exception("Input q cannot be null");
            int DOF_state = this.System.Get_DOF_state();
            double[] q_new = new double[DOF_state];
            for (int i = 0; i < DOF_state; ++i)
                q_new[i] = q[i];
            Node x = Node.SpawnGraphicNode();
            x.set_Value(new NodeData_RRT(q_new, 0));
            return x;
        }
        public override Node SpawnNode_As(double[] q)
        {
            if (q.Length != this.System.Get_DOF_state())
                throw new Exception("q.length != DOF_state. in RRT.SpawnNode_As");
            Node x = Node.SpawnGraphicNode();
            x.set_Value(new NodeData_RRT(q, 0));
            return x;
        }
        public override Edge SpawnEdge()
        {
            Edge e = Edge.SpawnGraphicEdge();
            e.set_Value(new EdgeData_RRT());
            return e;
        }
        public override Edge SpawnEdge(Node entry, Node exit)
        {
            Edge e = Edge.SpawnGraphicEdge(entry, exit);
            e.set_Value(new EdgeData_RRT());
            return e;
        }
        public Edge SpawnEdge(Node entry, Node exit, double c_inc, double[] ctrl_inc)
        {
            Edge e = Edge.SpawnGraphicEdge(entry, exit);
            e.set_Value(new EdgeData_RRT(c_inc, ctrl_inc));
            return e;
        }
        public RRT(RoboticSystemComponent system, VisualizationComponent visualizer, string motionPlanningConfigFile)
            : base(system, visualizer, motionPlanningConfigFile)
        {
        }

        public override void Execute(int n_iterations)
        {
            if (x_root == null)
                throw new Exception("x_root uninitialized. Forget to call SetRoot()?");

            int iterations_this_round = 0;
            while (iterations_this_round < n_iterations)
            {
                ++iterations_this_round;
                ++iterations_total;
                /*
                 ----------------- Sampling Phase -------------------
                 */
                double[] q_sample = this.Sampler.NextQ(SamplingComponent.SamplingMode.SAMPLING, this.System);
                /*
                 ---------- Nearest Neighbor Search Phase -----------
                 */
                double dist_nearest = double.MaxValue;
                Object obj_nearest = null;
                this.Vertices.NearestNeighbor_Approximate(q_sample, ref dist_nearest, ref obj_nearest);
                if (obj_nearest == null) continue;
                Node x_nearest = (Node)obj_nearest;
                /*
                 ----------------- Extension Phase ------------------
                 */

                //propagation
                double[] u_inc = new double[this.System.Get_DOF_control()];
                u_inc = this.Sampler.NextControl(SamplingComponent.SamplingMode.CONTROL, this.System);
                double t_inc = u_inc[u_inc.Length - 1];

                double[] q_new = new double[this.System.Get_DOF_state()];
                double cost_inc = 0;
                bool succ_propagation = this.System.Propagate(x_nearest.get_Value().get_q(), u_inc, t_inc, ref q_new, ref cost_inc);
                if (succ_propagation == false) continue;
                bool inRange = this.System.PointInRange(q_new);
                if (inRange == false) continue;

                //grow tree
                Node x_new = this.SpawnNode_As(q_new);
                x_new.get_Value().set_utility_c(x_nearest.get_Value().get_utility_c() + cost_inc);
                Edge e_new = this.SpawnEdge(x_nearest, x_new, cost_inc, u_inc);
                x_new.add_ParentEdge(e_new);
                x_nearest.add_ChildEdge(e_new);
                this.Vertices.AddPoint(x_new.get_Value().get_q(), x_new);

                //goal check
                bool DoGoalCheck = true;
                if (DoGoalCheck)
                {
                    if (GoalReached(x_new.get_Value().get_q()))
                    {
                        if (this.solution != null)
                        {
                            if (x_new.get_Value().get_utility_c() < this.solution.GetCost())
                                this.solution = new Trajectory(x_new);
                        }
                        else
                            this.solution = new Trajectory(x_new);
                        break;
                    }
                }

                //miscellaneous
                bool DoMiscellaneous = false;
                if (DoMiscellaneous)
                {
                    this.incrementalNodeArray.Add(x_new);
                    if (this.incrementalNodeArray.Count > 1000)
                    {
                        this.Plot_incrementally();
                        Thread.Sleep(0);
                    }
                }
            }
        }

        public override string ToString()
        {
            return "\n\t<Algorithm_Type>Standard_RRT</Algorithm_Type>" +
                    "\n\t<Algorithm_Parameters></Algorithm_Parameters>" + "\n";
        }

        public void Test1()
        {

            this.SetRoot(new double[this.System.Get_DOF_state()]);

            for (int i = 0; i < 10000; ++i)
            {
                double[] q_sample = this.Sampler.NextQ(SamplingComponent.SamplingMode.SAMPLING, this.System);
                Node x_sample = this.SpawnNode_As(q_sample);
                bool add_succ = this.Vertices.AddPoint(x_sample.get_Value().get_q(), x_sample);
                if (add_succ == false)
                    Debugger.Break();
            }
            Plot();
            Vertices.EmptyAllDataObjects();
            global::System.GC.Collect();
        }

        public void Plot_incrementally()
        {
            //Visualizer.PlotVerticesWithCost(this.incrementalNodeArray);
            this.Visualizer.ClearCanvas();
            this.Visualizer.PlotEdges(this.incrementalNodeArray);
            this.incrementalNodeArray.Clear();
        }

        public override bool GoalReached(double[] q_test)
        {
            if (r_goal <= 0)
                throw new NotImplementedException("Forget to call SetGoal and SetGoalTolerance first?");
            double distance2 = this.System.Distance2_Topological(q_goal, q_test);

            double distance=Math.Sqrt(distance2);

            if (distance2 <= r_goal * r_goal)
                return true;
            else
                return false;
        }
        public double GetGoalTolerance()
        {
            return this.r_goal;
        }
        public void SetGoalTolerance(double radius)
        {
            this.r_goal = radius;
        }
        override public void SetGoal(double[] q_goal)
        {
            this.q_goal = q_goal;
        }
        override public void Test()
        {
            double[] x_start = new double[6];
            double[] x_tmp = new double[6];
            x_start[0] = 0;            //x
            x_start[1] = 0;             //y
            x_start[2] = 0;   //thetta
            x_start[3] = 2;             //s
            x_start[4] = 2;            //v: lateral speed
            x_start[5] = 0;           //w
            this.SetRoot(x_start);

            double[] ctrl = new double[2];
            for (int i = 0; i < 1000; ++i)
            {
                double cost = 0;
                this.System.Propagate(x_start, ctrl, 0.1, ref x_tmp, ref cost);
                MotionPlanningUtils.vector_set(x_tmp, ref x_start);
                ((Visualizer_SimplifiedPlanarPhysicalRobots)this.Visualizer).DrawPhysicalRobot(((System_SimplifiedPlanarPhysicalRobot)System), x_tmp);
            }
        }
    }
}
